#include "StdAfx.h"

#include "line.h"
#include "vector3.h"

#include "ExUtilityFunction.h"
#include "ExHullMesh.h"
#include "3rd_part/xml/TiXmlUtility.h"
#include "ExPortal.h"

const float Normal_TOLERANCE = 0.1f;
const float Distace_TOLERANCE = 0.1f;

//used for search index on the convex hull
ExHullMesh::BiIndex::BiIndex()
{
}

ExHullMesh::BiIndex::BiIndex(uint32 i1, uint32 i2)
	:i1(i1),i2(i2)
{
}

uint32& ExHullMesh::BiIndex::operator[](uint32 index)
{
	assert( index < 2 );
	return index == 0? i1 : i2 ;
}

const uint32& ExHullMesh::BiIndex::operator[](uint32& index)
{
	assert( index < 2 );
	return index == 0? i1 : i2 ;
}


ExHullMesh::TriIndex::TriIndex()
{
}

ExHullMesh::TriIndex::TriIndex(uint32 i1, uint32 i2, uint32 i3)
	:i1(i1),i2(i2),i3(i3)
{
}

uint32& ExHullMesh::TriIndex::operator[](uint32 index)
{
	assert( index < 3 );
	switch( index )
	{
	case 0:
		return i1;
	case 1:
		return i2;
	case 2:
		return i3;
	default:
		assert( false );
	}
	return i1;
}

const uint32& ExHullMesh::TriIndex::operator[](uint32& index)
{
	assert( index < 3 );
	switch( index )
	{
	case 0:
		return i1;
	case 1:
		return i2;
	case 2:
		return i3;
	default:
		assert( false );
	}
	return i1;
}


ExHullMesh::ExHullMesh(void)
{
}


ExHullMesh::~ExHullMesh(void)
{
}


/**
 *	This method saves all of the boundaries
 *	and portals to the given data section.
 *
 *	It outputs a series of <boundary> tags,
 *	and if a boundary has an associated portal,
 *	the boundary tag will have a <portal> child.
 */
bool ExHullMesh::Save( TiXmlNode* pContentSection )
{
	//Convert all vertices etc. to correct units + orientation.
	BVVector conVertices = m_vVertices;
	// Make sure the hull in convex
	std::vector<TriIndex> faces;
	if ( !this->_MakeConvex( conVertices, faces ) )
	{
		::MessageBox(	NULL,
						_T("Unable to create custom hull.  Custom hull most contain at least four non-colinear and non-coplanar vertices!"),
						_T("Custom hull error!"),
						MB_ICONERROR );
		return false;
	}

	//Convert our triangular mesh into boundary planes
	//and associated portals.
	this->_ExtractPlanesAndPortals( conVertices, faces );

	// Need to track the number of portals added since the converting
	// of the custom hull to a convex hull may have moved the hull where
	// a portal was previously defined.

	// the amounts of the portals must be less than the boundary
	uint32 uPortalsAdded = m_conPortals.size();
	Boundaries::iterator pos = m_conPlanes.begin();
	for ( int iPortalIdx = 0 ;pos != m_conPlanes.end(); ++pos, ++iPortalIdx )
	{
		Common::Plane plane = *pos;
		
		TiXmlNode* pBoundary = new TiXmlElement( "boundary" );
		pContentSection->InsertEndChild(*pBoundary);

		TiXmlUtility::WritePlane( ExUtilityFunction::MaxToDx( ExUtilityFunction::ApplyUnitScale(plane) ), pBoundary);

		PortalMap::iterator itrPortal = m_conPortals.find(iPortalIdx);
		if ( itrPortal != m_conPortals.end() )
		{
			ExPortal* pPortal = itrPortal->second;
			pPortal->Save( pBoundary );	
			uPortalsAdded--;
		}
	}

	// Check that all portals were added
	if ( uPortalsAdded != 0)
	{
		::MessageBox(	0,
						_T("One or more portals defined using hull materials will not be exported.\n")
						_T("The hull was converted from concave to convex at that location!"),
						_T("Portal error!"),
						MB_ICONERROR );
		return false;
	}

	return true;
}

/**
 *	This method creates a convex hull from a vector of vertices.  It will determine if the vertices
 *	describe a convex or a concave hull.  If they describe a concave hull, a convex hull that
 *	enclose this cancave hull is created and a warning is displayed.
 *
 *	@param vertices		The list of hull vertices.
 *	@returns			Convex hull created
 */
bool ExHullMesh::_MakeConvex( BVVector& vertices, std::vector<TriIndex>& faces ) const
{
	// Flag used to record whether the hull was initially concave
	bool isConcave = false;

	// Initialise vectors
	std::vector<uint32> verts;			// The unresolved verts
	for ( size_t i = 0; i < vertices.size(); i++ )
		// Initialise the verts list with all vertices
		verts.push_back( i );

	// Create the maximal volume tetrahedron
	if ( !this->mcCreateMaxTetrahedron( vertices, verts, faces ) )
		// Cannot created convex hull
		return false;

	// Add next vert
	while ( verts.size() )
	{
		// Remove any points inside the volume
		if ( this->mcRemoveInsideVertices( vertices, verts, faces ) )
		{
			isConcave = true;

			if (verts.size() == 0)
				break;
		}

		// Find the vert that is furthest from the hull
		uint32 vertFurthestIndex = this->mcFindNextFurthestVertToHull( vertices, verts, faces );

		// Create new faces using this furthest vertex
		this->mcExpandHull( vertFurthestIndex, vertices, verts, faces );
	}

	// Check if hull was concave
	if ( isConcave )
		::MessageBox(	0,
						_T("The custom hull is concave or has colinear/coplanar vertices.  The exported hull may have changed shape to make it convex!"),
						_T("Custom hull warning!"),
						MB_ICONWARNING );

	// Successfully created convex hull
	return true;
}


/**
 *	This method creates the Maximal Volume Tetrahedron (MVT).  The MVT is the maximum volume
 *	enclosed by any four points of the vertices point cloud.
 *
 *	@param vertices		The vertices to be enclosed in the convex hull.
 *	@param verts		The verts outside the convex hull.
 *	@param faces		The faces of the convex hull.
 *	@returns			MVT created
 */
bool	ExHullMesh::mcCreateMaxTetrahedron	(	const BVVector& vertices, std::vector<uint32>& verts,
										std::vector<TriIndex>& faces, const float epsilon ) const
{
	// Must be at least four vertices
	if ( (vertices.size() < 4) || (verts.size() < 4) )
		return false;

	// Find the six points at the extremes of each axis
	float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
	float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;
	uint32 minX_index, minY_index, minZ_index,
		 maxX_index, maxY_index, maxZ_index;
	std::vector<uint32>::iterator it;
	for ( it = verts.begin(); it != verts.end(); it++ )
	{
		if ( vertices[*it].pos[0] < minX ) { minX = vertices[*it].pos[0]; minX_index = *it; }
		if ( vertices[*it].pos[1] < minY ) { minY = vertices[*it].pos[1]; minY_index = *it; }
		if ( vertices[*it].pos[2] < minZ ) { minZ = vertices[*it].pos[2]; minZ_index = *it; }
		
		if ( vertices[*it].pos[0] > maxX ) { maxX = vertices[*it].pos[0]; maxX_index = *it; }
		if ( vertices[*it].pos[1] > maxY ) { maxY = vertices[*it].pos[1]; maxY_index = *it; }
		if ( vertices[*it].pos[2] > maxZ ) { maxZ = vertices[*it].pos[2]; maxZ_index = *it; }
	}

	// Find the longest extent out of the three axes.  This will form one of
	// the edges of the MVT.  Remove these indices from verts
	BiIndex edge;
	Common::Line3D* pLongestAxisLine = NULL;
	Common::Vector3 rLinePoint1;
	Common::Vector3 rLinePoint2;

	if ( (maxX - minX) > (maxY - minY) )
	{
		if ( (maxX - minX) > (maxZ - minZ) )
		{
			edge = BiIndex(minX_index, maxX_index);
			ExUtilityFunction::Convert( vertices[minX_index].pos, rLinePoint1 );
			ExUtilityFunction::Convert( vertices[maxX_index].pos, rLinePoint2 );
		}
		else
		{
			edge = BiIndex(minZ_index, maxZ_index);
			ExUtilityFunction::Convert( vertices[minZ_index].pos, rLinePoint1 );
			ExUtilityFunction::Convert( vertices[maxZ_index].pos, rLinePoint2 );
		}
	}
	else
	{
		if ( (maxY - minY) > (maxZ - minZ) )
		{
			edge = BiIndex(minY_index, maxY_index);
			ExUtilityFunction::Convert( vertices[minY_index].pos, rLinePoint1 );
			ExUtilityFunction::Convert( vertices[maxY_index].pos, rLinePoint2 );
		}
		else
		{
			edge = BiIndex(minZ_index, maxZ_index);
			ExUtilityFunction::Convert( vertices[minZ_index].pos, rLinePoint1 );
			ExUtilityFunction::Convert( vertices[maxZ_index].pos, rLinePoint2 );
		}
	}
	pLongestAxisLine = new Common::Line3D(rLinePoint1, rLinePoint2);

	this->mcRemoveVertex( edge.i1, vertices, verts );
	this->mcRemoveVertex( edge.i2, vertices, verts );

	// Must be at least two verts left and line must be valid (i.e. cannot be
	// constructed from coincident points)
	if ( (verts.size() < 2) || !pLongestAxisLine->IsValid() )
		return false;

	// Find the point with the greatest perpindicular distance from edge.
	// Remove the index from verts
	float fFarthestDist = -FLT_MAX;
	uint32 fFarthestPointIndex;
	Common::Vector3 vDistToLine;
	
	for ( it = verts.begin(); it != verts.end(); it++ )
	{
		ExUtilityFunction::Convert(vertices[*it].pos, vDistToLine);
		float fDist = pLongestAxisLine->DistanceToPoint(vDistToLine);

		if ( fDist > fFarthestDist )
		{
			fFarthestDist = fDist;
			fFarthestPointIndex = *it;
		}
	}
	this->mcRemoveVertex( fFarthestPointIndex, vertices, verts );

	// Must be at least one vert left and must have a point that is not colinear to line
	if (  (verts.size() < 1) || (fFarthestDist < epsilon) )
		return false;

	TriIndex triangle(edge.i1, edge.i2, fFarthestPointIndex);

	// Create a plane from the three points
	Common::Vector3 vPoint1, vPoint2, vPoint3;
	ExUtilityFunction::Convert(vertices[triangle.i1].pos, vPoint1);
	ExUtilityFunction::Convert(vertices[triangle.i2].pos, vPoint2);
	ExUtilityFunction::Convert(vertices[triangle.i3].pos, vPoint3);

	Common::Plane plane( vPoint1, vPoint2, vPoint3 );
	// Find the point that is furthest from the plane
	fFarthestDist = -FLT_MAX;
	Common::Vector3 vDistToPlane;
	for ( it = verts.begin(); it != verts.end(); it++ )
	{
		ExUtilityFunction::Convert(vertices[*it].pos, vDistToPlane);
		float tempDist = plane.getDistance(vDistToPlane);
		if ( abs( tempDist ) > fFarthestDist )
		{
			fFarthestDist = abs( tempDist );
			fFarthestPointIndex = *it;
		}
	}
	this->mcRemoveVertex( fFarthestPointIndex, vertices, verts );

	// Must have a point that is not coplanar to plane
	if ( fFarthestDist < epsilon )
		return false;

	// Create the MVT from the four identified points
	ExUtilityFunction::Convert(vertices[*it].pos, vDistToPlane);
	fFarthestDist = plane.getDistance(vDistToPlane);
	if ( fFarthestDist > 0 )
	{
		// Reverse the order of the verts when adding the face
		faces.push_back( TriIndex( triangle.i3, triangle.i2, triangle.i1 ) );

		// Add remaining 3 faces in reverse order
		faces.push_back( TriIndex( triangle.i1, triangle.i2, fFarthestPointIndex ) );
		faces.push_back( TriIndex( triangle.i2, triangle.i3, fFarthestPointIndex ) );
		faces.push_back( TriIndex( triangle.i3, triangle.i1, fFarthestPointIndex ) );
	}
	else
	{
		// Add as defined
		faces.push_back( triangle );

		// Add remaining 3 faces
		faces.push_back( TriIndex( triangle.i3, triangle.i2, fFarthestPointIndex ) );
		faces.push_back( TriIndex( triangle.i2, triangle.i1, fFarthestPointIndex ) );
		faces.push_back( TriIndex( triangle.i1, triangle.i3, fFarthestPointIndex ) );
	}

	// MVT created successfully
	return true;
}

/**
 *	This method removes a vertex from the verts list as well as all vertices with the
 *	same position within the epsilon threshold.
 *
 *	@param vertIndex	The index of the vertex to remove from verts.
 *	@param vertices		The vertices to be enclosed in the convex hull.
 *	@param verts		The verts outside the convex hull.
 *	@param epsilon		The threshold determining if two vertices are coincident.
 */
void ExHullMesh::mcRemoveVertex( const uint32 vertIndex, const BVVector& srcVertices, std::vector<uint32>& modVerts, const float epsilon ) const
{
	// Remove all vertices at this position
	Point3 vRemovePos = srcVertices[vertIndex].pos;

	for ( std::vector<uint32>::iterator it = modVerts.begin(); it != modVerts.end(); )
	{
		Point3 vDiff = srcVertices[*it].pos - vRemovePos;
		if ( vDiff.Length() < epsilon )
			it = modVerts.erase( it );
		else
			it++;
	}
}


/**
 *	This method removes any vertices contained inside the convex hull.
 *
 *	@param vertices		The vertices to be enclosed in the convex hull.
 *	@param verts		The verts outside the convex hull.
 *	@param faces		The faces of the convex hull.
 *	@returns			Removed one or more vertices.
 */
bool ExHullMesh::mcRemoveInsideVertices( const BVVector& vertices, std::vector<uint32>& verts,
										const std::vector<TriIndex>& faces ) const
{
	bool vertsRemoved = false;

	// Loop through the verts
	std::vector<uint32>::iterator it;
	for ( it = verts.begin(); it != verts.end(); )
	{
		// Loop through the faces, checking the vert against each face.
		// If the vert is on the outside of any face, it must be outside
		// the convex hull
		bool inside = true;

		std::vector<TriIndex>::const_iterator face_it;
		Common::Plane coplanar;
		Common::Vector3 point1, point2, point3;
		Common::Vector3 vTestPoint;

		for ( face_it = faces.begin(); face_it != faces.end(); face_it++ )
		{
			// If on positive side, must be outside
			ExUtilityFunction::Convert( vertices[face_it->i1].pos, point1 );
			ExUtilityFunction::Convert( vertices[face_it->i2].pos, point2 );
			ExUtilityFunction::Convert( vertices[face_it->i3].pos, point3 );
			ExUtilityFunction::Convert( vertices[*it].pos, vTestPoint );

			coplanar.redefine( point1, point2, point3 );
			Common::Plane::Side eTestPointSide = coplanar.getSide( vTestPoint );
			if( eTestPointSide == Common::Plane::POSITIVE_SIDE )
			{
				inside = false;
				break;
			}
		}

		if (inside)
		{
			vertsRemoved = true;
			this->mcRemoveVertex( *it, vertices, verts );
		}
		else
		{
			it++;
		}
	}

	return vertsRemoved;
}

bool ExHullMesh::_IsPortal( uint32 materialIndex )
{
	return false;
}

/**
 *	This method goes through all the triangles in the hull,
 *	and extracts a set of plane equations out.
 *
 *	It also stores the indices of planes that are designated
 *	portals via their materials.
 */

class BoundaryEqual
{
public:
	BoundaryEqual( Common::Plane& rTarget )
		:m_rTarget(rTarget)
	{
	}
	~BoundaryEqual(void)
	{
	}

	bool operator() ( const Common::Plane& plane ) const
	{
		if( ::fabs(m_rTarget.normal.dotProduct( plane.normal )) < Normal_TOLERANCE && ::fabs( m_rTarget.d - m_rTarget.d ) < Distace_TOLERANCE )
			return true;
		return false;
	}

private:

	 Common::Plane& m_rTarget;
};

void ExHullMesh::_ExtractPlanesAndPortals( BVVector& vertices, std::vector<TriIndex>& faces )
{
	m_conPlanes.clear();
	m_conPortals.clear();

	std::vector<TriIndex>::iterator face_it;
	for ( face_it = faces.begin(); face_it != faces.end(); face_it++ )
	{
		// Add face plane (note that the winding needs to be reversed)
		Common::Plane rPlane(
			Common::Vector3(vertices[face_it->i3].pos),
			Common::Vector3(vertices[face_it->i2].pos),
			Common::Vector3(vertices[face_it->i1].pos) );

		if( std::find_if( m_conPlanes.begin(), m_conPlanes.end(), BoundaryEqual(rPlane) ) == m_conPlanes.end() )
			m_conPlanes.push_back(rPlane);
	}

	// Loop through the original triangles searching for portal entries.  It is
	// safe to add them even though the hull may have been altered in makeConvex
	// since the portals will be tested against the hull before saving
	TriangleVector::iterator it = m_vTriangles.begin();
	TriangleVector::iterator end = m_vTriangles.end();

	Common::Vector3 point1, point2, point3;

	while ( it != end )
	{
		Triangle& tri = *it++;
		ExUtilityFunction::Convert( vertices[tri.index[0]].pos, point1 );
		ExUtilityFunction::Convert( vertices[tri.index[1]].pos, point2 );
		ExUtilityFunction::Convert( vertices[tri.index[2]].pos, point3 );
		
		Common::Plane rPlane( point1, point2, point3 );

		Boundaries::iterator itrBoundary = std::find_if( m_conPlanes.begin(), m_conPlanes.end(), BoundaryEqual(rPlane) );
		int planeIndex = std::distance( m_conPlanes.begin(), itrBoundary );

		if (_IsPortal( tri.materialIndex ))
		{
			if ( m_conPortals.find(planeIndex) == m_conPortals.end() )
			{
				ExPortal* pPortal = new ExPortal;
				m_conPortals.insert( std::make_pair(planeIndex,pPortal) );

				/*
				//Set the name of the portal, based on the material name.
				Material& mat = materials_[tri.materialIndex];
				std::string matName = toLower(mat.identifier);
				int nPos = matName.find("_portal");
				matName = matName.substr(0,nPos);
				const char *dataName = vpp->propDataFromPropName(matName.c_str());
				if ( dataName )
					vpp->name(dataName);
					*/
			}

			ExPortal* pExistPortal = m_conPortals.find(planeIndex)->second;

			pExistPortal->AddVertex(point1, false);
			pExistPortal->AddVertex(point2, false);
			pExistPortal->AddVertex(point3, false);
		}
	}


	//Create convex hulls from all the VisualPortals
	PortalMap::iterator pit = m_conPortals.begin();
	while ( pit != m_conPortals.end() )
	{
		ExPortal* pPortal = pit->second;
		pPortal->CreateConvexHull();
		pit++;
	}
}

/**
 *	This method determines the edge horizon for vertIndex and expands the convex hull from
 *	the edge horizon to vertIndex.
 *
 *	@param vertIndex	The index of the vertex to add to the convex hull.
 *	@param vertices		The vertices to be enclosed in the convex hull.
 *	@param verts		The verts outside the convex hull.
 *	@param faces		The faces of the convex hull.
 */
void ExHullMesh::mcExpandHull( const uint32 vertIndex, const BVVector& vertices, std::vector<uint32>& verts, std::vector<TriIndex>& faces ) const
{
	// Determine the front-facing and back-facing faces w.r.t. vertIndex
	std::vector<uint32> frontFacing;
	std::vector<uint32> backFacing;

	Common::Plane coplanar;
	Common::Vector3 point1, point2, point3;
	Common::Vector3 vTestPoint;

	for ( uint32 face_it = 0; face_it != faces.size(); face_it++ )
	{
		// If on positive side, must be outside
		ExUtilityFunction::Convert( vertices[faces[face_it].i1].pos, point1 );
		ExUtilityFunction::Convert( vertices[faces[face_it].i2].pos, point2 );
		ExUtilityFunction::Convert( vertices[faces[face_it].i3].pos, point3 );

		ExUtilityFunction::Convert( vertices[vertIndex].pos, vTestPoint );

		//form a plane
		coplanar.redefine( point1, point2, point3 );

		Common::Plane::Side eTestPointSide = coplanar.getSide( vTestPoint );
		if( eTestPointSide == Common::Plane::POSITIVE_SIDE )
			frontFacing.push_back( face_it );
		else if( eTestPointSide == Common::Plane::NEGATIVE_SIDE )
			backFacing.push_back( face_it );
	}

	// Find horizon vertices
	std::vector<uint32> horizonVerts;

	// Cover the special case of a single front facing triangle
	if ( frontFacing.size() == 1 )
	{
		horizonVerts.push_back( faces[frontFacing[0]].i1 );
		horizonVerts.push_back( faces[frontFacing[0]].i2 );
		horizonVerts.push_back( faces[frontFacing[0]].i3 );
		faces.erase( faces.begin() + frontFacing[0], faces.begin() + frontFacing[0] + 1 );
		this->mcRemoveVertex( vertIndex, vertices, verts );
		this->mcAddFaces( vertIndex, horizonVerts, faces );
		return;
	}

	// We need to be able to handle the general case of many front facing triangle,
	// some of which do not touch the edge horizon, and others that touch the edge
	// horizon with one or more vertices

	// Any vertices that appears in both the front facing and back facing lists must lie on the
	// horizon.
	for ( size_t i = 0; i < frontFacing.size(); i++ )
	{
		for ( size_t j = 0; j < backFacing.size(); j++ )
		{
			std::vector<uint32>::iterator result;

			for ( int k = 0; k < 3; k++ )
			{
				if (faces[frontFacing[i]][k] == faces[backFacing[j]].i1 ||
					faces[frontFacing[i]][k] == faces[backFacing[j]].i2 ||
					faces[frontFacing[i]][k] == faces[backFacing[j]].i3)
				{
					result = std::find( horizonVerts.begin(), horizonVerts.end(), faces[frontFacing[i]][k] );
					if ( result == horizonVerts.end() )
						horizonVerts.push_back( faces[frontFacing[i]][k] );
				}
			}
		}
	}

	// Order the vertices that lie on the horizon in a clockwise order when viewed from the
	// vertIndex's position

	// Find a front facing triangle with two or more vertices on the edge horizon and with
	// a unique vert that no other front facing triangle is indexing
	std::vector<uint32> sortedHorizonVerts;
	int index;
	int count = 0;
	bool indexFound[] = { false, false, false };
	for ( size_t i = 0; i < frontFacing.size(); i++ )
	{
		indexFound[0] = false; indexFound[1] = false; indexFound[2] = false;
		count = 0;

		// Need to test each horizon vertex against each face vertex
		for ( size_t j = 0; j < horizonVerts.size(); j++ )
		{
			for ( int k = 0; k < 3; k++ )
			{
				if (horizonVerts[j] == faces[frontFacing[i]][k])
				{
					count++;
					indexFound[k] = true;
				}
			}
		}

		if ( count == 2 )
		{
			index = i;
			break;
		}

		if ( count == 3 )
		{
			indexFound[0] = false; indexFound[1] = false; indexFound[2] = false;

			// For each vertex of i
			for ( int j = 0; j < 3; j++ )
			{
				// For each triangle in frontFacing
				for ( size_t k = 0; k < frontFacing.size(); k++ )
				{
					// Don't check against self
					if ( k == i )
						continue;

					if (faces[frontFacing[i]][j] == faces[frontFacing[k]].i1 ||
						faces[frontFacing[i]][j] == faces[frontFacing[k]].i2 ||
						faces[frontFacing[i]][j] == faces[frontFacing[k]].i3)
					{
						indexFound[j] = true;
						break;
					}
				}
			}

			if ( !indexFound[0] || !indexFound[1] || !indexFound[2] )
			{
				index = i;
				break;
			}
		}
	}
	// This assert ensures that at least two of the face vertices are on the horizon
	ASSERT ( count > 1 && _T("HullMesh::mcExpandHull - Failed to find triangle on horizon edge") );

	// Add the verts to the sortedHorizonVerts in acsending order
	for ( int i = 0; i < 3; i++ )
	{
		if (!indexFound[i])
		{
			if (count == 2)
			{
				sortedHorizonVerts.push_back(faces[frontFacing[index]][i+1]);
				sortedHorizonVerts.push_back(faces[frontFacing[index]][i+2]);
				break;
			}
			else
			{
				sortedHorizonVerts.push_back(faces[frontFacing[index]][i+2]);
				sortedHorizonVerts.push_back(faces[frontFacing[index]][i]);
				sortedHorizonVerts.push_back(faces[frontFacing[index]][i+1]);
				break;
			}
		}
	}

	// Find the front facing triangles with two (or more) verts on the horizon, one of which is
	// sortedHorizonVerts.back() and the next that is not already in sortedHorizonVerts.  Also
	// Make sure this edge is and external edge of the front facing triangles

	// While there are more vertices to account for
	while ( sortedHorizonVerts.size() != horizonVerts.size() )
	{
		bool found = false;

		// Loop through the front faces
		for ( size_t j = 0; j < frontFacing.size(); j++ )
		{
			// Loop through the vertices of the jth front face
			for ( int k = 0; k < 3; k++ )
			{
				// If the kth vert is the last vert in sortedHorizonVerts
				if (faces[frontFacing[j]][k] == sortedHorizonVerts.back())
				{
					bool inSortedList = false;

					// Cycle through sorted verts
					for ( size_t l = 0; l < sortedHorizonVerts.size(); l++ )
					{
						// Check if equal
						if (faces[frontFacing[j]][k+1] == sortedHorizonVerts[l])
						{
							inSortedList = true;
							break;
						}
					}

					if (!inSortedList)
					{
						// Check if connecting edge in internal
						if ( !this->mcIsInternalEdge( faces, frontFacing, sortedHorizonVerts.back(), faces[frontFacing[j]][k+1]) )
						{
							sortedHorizonVerts.push_back(faces[frontFacing[j]][k+1]);
							found = true;
							break;
						}
					}
				}
			}

			if (found)
				break;
		}

		if (!found)
			break;
	}

	// Delete all the front faces from faces in reverse order
	std::sort(frontFacing.begin(), frontFacing.end());
	std::reverse(frontFacing.begin(), frontFacing.end());
	for ( size_t i = 0; i < frontFacing.size(); i++ )
	{
		faces.erase( faces.begin() + frontFacing[i], faces.begin() + frontFacing[i] + 1 );
	}

	// Add the new faces
	this->mcAddFaces( vertIndex, sortedHorizonVerts, faces );

	// Delete the vertIndex
	this->mcRemoveVertex( vertIndex, vertices, verts );
}


bool ExHullMesh::mcIsInternalEdge(std::vector<TriIndex>& faces, const std::vector<uint32>& frontFacing,
								const uint32 vertIndex1, const uint32 vertIndex2) const
{
	// Check if the edge is used by a single front facing triangle
	int edgeUseCount = 0;
	for ( size_t i = 0; i < frontFacing.size(); i++ )
	{
		for ( int j = 0; j < 3; j++ )
		{
			if (	(vertIndex1 == faces[frontFacing[i]][j] && vertIndex2 == faces[frontFacing[i]][j+1]) ||
					(vertIndex2 == faces[frontFacing[i]][j] && vertIndex1 == faces[frontFacing[i]][j+1]) )
				edgeUseCount++;
		}
	}

	if (edgeUseCount > 1)
		return true;
	else
		return false;
}


/**
 *	This method adds a triangle fan with base vertIndex around the closed loop
 *	of vert defined by horizonVerts.
 *
 *	@param vertIndex	The index of the vertex being added to the convex hull.
 *	@param horizonVerts	The list of vertices on the edge horizon.
 *	@param faces		The faces of the convex hull.
 */
void ExHullMesh::mcAddFaces( const uint32 vertIndex, const std::vector<uint32>& horizonVerts,
										std::vector<TriIndex>& faces ) const
{
	for ( size_t i = 0; i < horizonVerts.size(); i++ )
	{
		faces.push_back( TriIndex( horizonVerts[i], horizonVerts[(i + 1) % horizonVerts.size()], vertIndex ) );
	}
}


/**
 *	This method returns the index of the next vertex to be add to the convex hull.
 *
 *	@param vertices		The vertices to be enclosed in the convex hull.
 *	@param verts		The verts outside the convex hull.
 *	@param faces		The faces of the convex hull.
 *	@returns			The index of the next vertex to be added.
 */
uint32 ExHullMesh::mcFindNextFurthestVertToHull( const BVVector& vertices, const std::vector<uint32>& verts,
										const std::vector<TriIndex>& faces ) const
{
	// Find the point with the greatest distance from any face
	float fFarthestDist = -FLT_MAX;
	uint32 fFarthestPointIndex;
	Common::Vector3 vPoint;

	// Loop through the verts
	std::vector<uint32>::const_iterator vert_it;
	for ( vert_it = verts.begin(); vert_it != verts.end(); vert_it++ )
	{
		// Loop through the faces, checking the distance of the vert against
		// each face.  If the distance is greater than the current max, set as
		// next vert
		std::vector<TriIndex>::const_iterator face_it;
		Common::Plane coplanar;
		Common::Vector3 point1, point2, point3;
		Common::Vector3 vTestPoint;

		for ( face_it = faces.begin(); face_it != faces.end(); face_it++ )
		{
			// If on positive side, must be outside
			ExUtilityFunction::Convert( vertices[face_it->i1].pos, point1 );
			ExUtilityFunction::Convert( vertices[face_it->i2].pos, point2 );
			ExUtilityFunction::Convert( vertices[face_it->i3].pos, point3 );
			ExUtilityFunction::Convert( vertices[*vert_it].pos, vTestPoint );

			coplanar.redefine( point1, point2, point3 );

			float fPointToFaceDist = coplanar.getDistance( vTestPoint );
			if( fPointToFaceDist > fFarthestDist )
			{
				fFarthestPointIndex = *vert_it;
				fFarthestDist = fPointToFaceDist;
			}
		}
	}

	return fFarthestPointIndex;
}


